/**
 *******************************************************************************
 * @file    source/HARDWARE/src/3901l0x.c
 * @brief   光流&激光测距雷达，3901-L0X
 * Date         Author          Notes
 * 02/03/2021   Althrone        MATEKSYS制造，MSP协议
 *******************************************************************************
 **/

/* Includes ------------------------------------------------------------------*/

#include "Matek.h"
#include "delay.h"

#include "filter.h"
#include "InertialNavigation.h"
#include "Attitude.h"
#include "Position.h"


#include "ADRC.h"


/* Private macro -------------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Public variables ----------------------------------------------------------*/

L0X3901_DataTypeDef LIDAR_DataStructure;
L0X3901_DataTypeDef OPFLOW_DataStructure;

extern uint8_t MSP_Buf[];
extern pos_control Sys_Pos_Control; 
extern yaw_control Yaw_control;
extern FLOAT_ANGLE Att_Angle;
extern uint32_t TIME_ISR_CNT; 

float LD_t1,LD_t2,LD_dt; 
float GL_t1,GL_t2,GL_dt;

Light_Lidar Sys_Light_Lidar;
Light_Flow Sys_Light_Flow;



/* Private variables ---------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
/* Public functions ----------------------------------------------------------*/

/**
 * @brief   读取3901-L0X模块所有参数
 * @param   LIDAR_DataStructure: 外部全局变量，读取到的数据提取到这个结构体
 * @param   frameHead: 帧头位置
 * @param   MSP_Fun: MSP协议帧功能指示，枚举类型
 **/
void L0X3901_AllDataRead(MSPFun_TypeDef MSP_Fun)
{
    if(MSP_Fun==MSP_Fun_RangeFinder)//按照激光测距解码
    {
        LIDAR_DataStructure.L0X3901_LidarQlty=MSP_Buf[8];//有效数据是5个字节，按照inav源码，有一个字节是质量，四个字节是距离数据
        int32_t tmp=0;
        for (uint8_t i = 0; i < 4; i++)
        {
            tmp|=(uint32_t)(MSP_Buf[i+9])<<(i*8);
			
        }
        if(tmp!=-1)
        {
			GL_t1 = 10000*TIME_ISR_CNT + TIM2->CNT;
			GL_dt = (GL_t1 -  GL_t2)*1e-6f;	
			GL_t2 = GL_t1;
			
            LIDAR_DataStructure.L0X3901_Distance=tmp;
	
			Sys_Light_Lidar.Now_Ralt = (0.1*LIDAR_DataStructure.L0X3901_Distance) - Sys_Light_Lidar.Init_alt;
			Sys_Light_Lidar.Now_Ralt *=  cos(Att_Angle.pit*DegtoRad) * cos(Att_Angle.rol*DegtoRad);
			
			float H = Sys_Light_Lidar.Now_Ralt;			
			RC_Filter_(0.1,H,&Sys_Light_Lidar.Now_Ralt);     //高度滤波
									
						
	        Sys_Light_Lidar.Velocity = (Sys_Light_Lidar.Now_Ralt -  Sys_Light_Lidar.Last_Ralt)/GL_dt; 
			Sys_Light_Lidar.Velocity += 0.7f*(Sys_Light_Lidar.Last_Velocity - Sys_Light_Lidar.Velocity);    //速度滤波
						
	        Sys_Light_Lidar.Last_Ralt = Sys_Light_Lidar.Now_Ralt;			
			Sys_Light_Lidar.Last_Velocity = Sys_Light_Lidar.Velocity; 
        }
    }
    else if(MSP_Fun==MSP_Fun_OpticalFlow)//按照光流解码
    {		
		LD_t1 = 10000*TIME_ISR_CNT + TIM2->CNT; 
		LD_dt = (LD_t1 -  LD_t2)*1e-6f;
				
        OPFLOW_DataStructure.L0X3901_OpFlowQlty=MSP_Buf[8];
        int32_t tmp=0;
        for (uint8_t i = 0; i < 4; i++)
        {
            tmp|=(uint32_t)(MSP_Buf[i+9])<<(i*8);
        }
        OPFLOW_DataStructure.L0X3901_MotionX=tmp;
		
		Sys_Light_Flow.MotionV_X = (- OPFLOW_DataStructure.L0X3901_MotionX); 		
		Sys_Light_Flow.MotionV_X = 0.75f * Sys_Light_Flow.MotionV_X + 0.25f  * Sys_Light_Flow.LMotionV_X;	
		
        tmp=0;
        for (uint8_t i = 0; i < 4; i++)
        {
            tmp|=(uint32_t)(MSP_Buf[i+13])<<(i*8);
        }
        OPFLOW_DataStructure.L0X3901_MotionY=tmp;
		
		Sys_Light_Flow.MotionV_Y = (- OPFLOW_DataStructure.L0X3901_MotionY); 		
		Sys_Light_Flow.MotionV_Y = 0.75f * Sys_Light_Flow.MotionV_Y + 0.25f * Sys_Light_Flow.LMotionV_Y;
				
//		if(Yaw_control.Ready == 1)		
//			Matek_PosFusion(LD_dt);
		
		Sys_Light_Flow.LMotionV_X = Sys_Light_Flow.MotionV_X;
		Sys_Light_Flow.LMotionV_Y = Sys_Light_Flow.MotionV_Y;
		LD_t2 = LD_t1;
    }
}

/* Private functions ---------------------------------------------------------*/

